	ORG     &H000		;Begin program at x000
Init:
	LOAD    Zero		;DISABLE EVERYTHING
	OUT     SONAREN     ; Disable sonar
	OUT     LVELCMD     ; Stop motors
	OUT     RVELCMD
	
	
	
	;OUTPUT to LCD "Toggle Switch 17, press Key1"
	;INSERT A KEY1 Spincall switch
	
	
	;Remove these waits when Keytoggle has been implemented
	CALL	Wait1
	CALL	Wait1
	CALL	Wait1
	CALL	Wait1
	CALL	Wait1

	LOAD	Zero
	OUT		TIMER
FORSPIN1: 				;spin forward slow
	LOAD 	Forward1		;GET THE WHEELS SPINNING FORWARD
	OUT		LVELCMD
	OUT		RVELCMD
	IN      LVEL        ; Read velocity of wheels
	STORE   Temp
	IN      RVEL
	ADD     Temp
	STORE	Speed
	OUT     SEVENSEG    ; Output sum of wheel velocities to 7-seg
	IN		TIMER
	ADDI	-20
	JNEG	FORSPIN1		;REPEAT FOR 2 SECONDS
	
	LOAD	Speed		;Compare Low range
	JZERO	NOSTARTFAIL
	SUB		RangeF1L
	JNEG	FAIL
	LOAD	Speed		;Compare High Range
	SUB		RangeF1H
	JPOS	FAIL
	
	
	
	LOAD	Zero
	OUT		TIMER
FORSPIN2: 				;spin forward slow
	LOAD 	Forward2		;GET THE WHEELS SPINNING FORWARD
	OUT		LVELCMD
	OUT		RVELCMD
	IN      LVEL        ; Read velocity of wheels
	STORE   Temp
	IN      RVEL
	ADD     Temp
	STORE	Speed
	OUT     SEVENSEG    ; Output sum of wheel velocities to 7-seg
	IN		TIMER
	ADDI	-20
	JNEG	FORSPIN2		;REPEAT FOR 2 SECONDS
	
	LOAD	Speed		;Compare Low range
	SUB		RangeF2L
	JNEG	FAIL
	LOAD	Speed		;Compare High Range
	SUB		RangeF2H
	JPOS	FAIL
	
	LOAD	Zero	
	OUT		TIMER
FORSPIN3: 				;spin forward slow
	LOAD 	Forward3		;GET THE WHEELS SPINNING FORWARD
	OUT		LVELCMD
	OUT		RVELCMD
	IN      LVEL        ; Read velocity of wheels
	STORE   Temp
	IN      RVEL
	ADD     Temp
	STORE	Speed
	OUT     SEVENSEG    ; Output sum of wheel velocities to 7-seg
	IN		TIMER
	ADDI	-20
	JNEG	FORSPIN3		;REPEAT FOR 2 SECONDS
	
	LOAD	Speed		;Compare Low range
	SUB		RangeF3L
	JNEG	FAIL
	LOAD	Speed		;Compare High Range
	SUB		RangeF3H
	JPOS	FAIL
	
	
	LOAD	Zero
	OUT		TIMER
REVSPIN1: 				;spin forward slow
	LOAD 	Reverse1		;GET THE WHEELS SPINNING FORWARD
	OUT		LVELCMD
	OUT		RVELCMD
	IN      LVEL        ; Read velocity of wheels
	STORE   Temp
	IN      RVEL
	ADD     Temp
	STORE	Speed
	OUT     SEVENSEG    ; Output sum of wheel velocities to 7-seg
	IN		TIMER
	ADDI	-20
	JNEG	REVSPIN1		;REPEAT FOR 2 SECONDS
	
	LOAD	Speed		;Compare Low range
	SUB		RangeR1L
	JPOS	FAIL
	LOAD	Speed		;Compare High Range
	SUB		RangeR1H
	JNEG	FAIL
	
	
	
	LOAD	Zero	
	OUT		TIMER
REVSPIN2: 				;spin forward slow
	LOAD 	Reverse2		;GET THE WHEELS SPINNING FORWARD
	OUT		LVELCMD
	OUT		RVELCMD
	IN      LVEL        ; Read velocity of wheels
	STORE   Temp
	IN      RVEL
	ADD     Temp
	STORE	Speed
	OUT     SEVENSEG    ; Output sum of wheel velocities to 7-seg
	IN		TIMER
	ADDI	-20
	JNEG	REVSPIN2		;REPEAT FOR 2 SECONDS
	
	LOAD	Speed		;Compare Low range
	SUB		RangeR2L
	JPOS	FAIL
	LOAD	Speed		;Compare High Range
	SUB		RangeR2H
	JNEG	FAIL
	
	
	
	LOAD	Zero	
	OUT		TIMER
REVSPIN3: 				;spin forward slow
	LOAD 	Reverse3		;GET THE WHEELS SPINNING FORWARD
	OUT		LVELCMD
	OUT		RVELCMD
	IN      LVEL        ; Read velocity of wheels
	STORE   Temp
	IN      RVEL
	ADD     Temp
	STORE	Speed
	OUT     SEVENSEG    ; Output sum of wheel velocities to 7-seg
	IN		TIMER
	ADDI	-20
	JNEG	REVSPIN3		;REPEAT FOR 2 SECONDS
	
	LOAD	Speed		;Compare Low range
	SUB		RangeR3L
	JPOS	FAIL
	LOAD	Speed		;Compare High Range
	SUB		RangeR3H
	JNEG	FAIL
	
	
	
	
;Clear again
	LOAD    Zero
	OUT     SONAREN     ; Disable sonar
	OUT     LVELCMD     ; Stop motors
	OUT     RVELCMD
	OUT		SEVENSEG
	JUMP	PASS
	
	
	
Wait1:
	OUT     TIMER		; One second pause subroutine
Wloop:
	IN      TIMER
	ADDI    -10
	JNEG    Wloop
	RETURN

PASS:
	;PASS STATEMENT, continue with self test
	LOAD	PA
	OUT		LCD
	JUMP	PASS
	
FAIL:
	;INSERT FAIL CODE, speed is way off
	LOAD	FA
	OUT		LCD
	JUMP	FAIL
	
NOSTARTFAIL:
	;when motors don't run at all
	JUMP	NOSTARTFAIL
	

; This is a good place to put variables
PA:			DW	&H1111
FA:			DW	&HFFFF
Temp:     DW 0
Speed:	  DW 0
Forward1:  DW 70
Reverse1:  DW -70
Forward2:  DW 100
Reverse2:  DW -100
Forward3:  DW 120
Reverse3:  DW -120
;RangeF1L:  DW  &H29
;RangeF1H:  DW  &H35
;RangeF2L:  DW  &H51
;RangeF2H:  DW  &H58
;RangeF3L:  DW  &H63
;RangeF3H:  DW  &H69
;RangeR1L:  DW  &HFFD0
;RangeR1H:  DW  &HFFC9
;RangeR2L:  DW  &HFFAC
;RangeR2H:  DW  &HFFA5
;RangeR3L:  DW  &HFF97
;RangeR3H:  DW  &HFF90

;Due to Battery volatility, ranges have been expanded
RangeF1L:  DW  &H21
RangeF1H:  DW  &H40
RangeF2L:  DW  &H43
RangeF2H:  DW  &H63
RangeF3L:  DW  &H55
RangeF3H:  DW  &H80
RangeR1L:  DW  &HFFD8
RangeR1H:  DW  &HFFC4
RangeR2L:  DW  &HFFB6
RangeR2H:  DW  &HFFA0
RangeR3L:  DW  &HFF9E
RangeR3H:  DW  &HFF85



; Having some constants can be very useful
Zero:     DW 0
One:      DW 1
Two:      DW 2
Three:    DW 3
Four:     DW 4
Five:     DW 5
Six:      DW 6
Seven:    DW 7
Eight:    DW 8
Nine:     DW 9
Ten:      DW 10
Forward:  DW 70
Reverse:  DW -70
EnSonar0: DW &B00000001
EnSonar1: DW &B00000010
EnSonar2: DW &B00000100
EnSonar3: DW &B00001000
EnSonar4: DW &B00010000
EnSonar5: DW &B00100000
EnSonar6: DW &B01000000
EnSonar7: DW &B10000000
EnSonars: DW &B11111111

; IO address space map
SWITCHES: EQU &H00  ; slide switches
LEDS:     EQU &H01  ; red LEDs
TIMER:    EQU &H02  ; timer, usually running at 10 Hz
XIO:      EQU &H03  ; pushbuttons and some misc. I/0
SEVENSEG: EQU &H04  ; seven-segment display (4-digits only)
LCD:      EQU &H06  ; primitive 4-digit LCD display
LPOS:     EQU &H80  ; left wheel encoder position (read only)
LVEL:     EQU &H82  ; current left wheel velocity (read only)
LVELCMD:  EQU &H83  ; left wheel velocity command (write only)
RPOS:     EQU &H88  ; same values for right wheel...
RVEL:     EQU &H8A  ; ...
RVELCMD:  EQU &H8B  ; ...
I2C_CMD:  EQU &H90  ; I2C module's CMD register,
I2C_DATA: EQU &H91  ; ... DATA register,
I2C_BUSY: EQU &H92  ; ... and BUSY register
SONAR:    EQU &HA0  ; base address for more than 16 registers....
DIST0:    EQU &HA8  ; the eight sonar distance readings
DIST1:    EQU &HA9  ; ...
DIST2:    EQU &HAA  ; ...
DIST3:    EQU &HAB  ; ...
DIST4:    EQU &HAC  ; ...
DIST5:    EQU &HAD  ; ...
DIST6:    EQU &HAE  ; ...
DIST7:    EQU &HAF  ; ...
SONAREN:  EQU &HB2  ; register to control which sonars are enabled
XPOS:     EQU &HC0  ; Current X-position (read only)
YPOS:     EQU &HC1  ; Y-position
THETA:    EQU &HC2  ; Current rotational position of robot (0-701)
